1. Field of the Invention
The present invention relates to a path planning device, a path planning method, and a computer program, which can plan a motion control of a robot or a motion route or path of a control target such as a finger tip of a multi-joint arm from present position and posture to target position and posture in a motion space, and more particularly, to a path planning device, a path planning method, and a computer program, which can correct a path of a control target for avoiding an obstruction into a smooth path for easy use in motion control or path control.
2. Description of the Related Art
For example, when a moving machine such as a robot or a finger tip as an end effector of a multi-joint arm is autonomously controlled in motion, a motion route or path from some position and posture (present position and posture: start) to another position and posture (target position and posture: goal) in a motion space should be planned. In the following description, the “route” and the “path” are both called “path”. Unless otherwise particularly specified, a path means a path of position and posture. That is, a path in a three-dimensional space means a six-degree-of-freedom path for describing a spatial posture of an object.
The invention is mainly directed to generating rapidly a smooth path of a robot or a multi-joint arm from the present position and posture to the target position and posture. The inventors consider that the following four requirements should be satisfied to accomplish the above-mentioned purpose:
(1) to acquire rapidly an initial position and posture path for accomplishing a target position and posture path, which is not the optimal solution;
(2) to acquire a smooth position and posture path;
(3) to avoid infiltration (collision or interference) between objects in a position and posture path; and
(4) to approach an ideal position and posture path (reflecting a designer's intention) as closely as possible.
It is difficult to acquire the optimal solution from infinite paths. Various path planning methods such as a potential method (for example, see “Introduction to Intelligent Robot—The Solution of the Motion Planning Problem”, written by ZYUNN OOTA and others (CORONA Publishing Co., Ltd., 2001)), a stochastic road map method (for example, see “Introduction to Intelligent Robot—The Solution of the Motion Planning Problem”, written by ZYUNN OOTA and others (CORONA Publishing Co., Ltd., 2001)), and a rapidly-exploring random tree (RRT) method (for example, see “Rapidly-Exploring Random Trees: A New Tool for Path Planning” (TR. 98-11, Computer Science Dpt., Iowa State University, October 1998), written by Steven M. Lavalle) have been suggested.
In the potential method, a path is planned by repeating a procedure of defining a potential field of an “attractive potential” attracting a control target to a target posture in a motion space and a “repulsive potential” repulsing the control target from an obstruction, calculating a gradient of the potential field in the present posture of the control target, and finely moving the control target in a direction in which the potential decreases.
For example, a path generating device has been suggested which calculates an attractive potential and a repulsive potential based on relative positional relations among a mobile object, an obstruction, and a target object using a map in a motion space generated on the basis of information on positions and shapes of the mobile object, the obstruction, and the target object, generates a combined potential as the sum of the potentials, searching a route in the map on the basis of the combined potential to determine whether a convergent position of the route search is a local minimum, generates a virtual potential obtained by increasing the potential of the convergent position by a predetermined value when the convergent position is the local minimum, and generates a motion path of the mobile object on the basis of the result of route search when the convergent position is a target position (for example, see Japanese Unexamined Patent Application Publication No. 2006-350776).
In the stochastic road map method, one posture is randomly selected from a configuration space, the posture is branched when it can move to a near posture, a random walk is taken from a certain posture in a region having many obstructions when a road map is not connected, the road map is extended, and a Dijkstra method as a mathematical programming method is applied to calculate the shortest distance in the road map.
For example, a lowest-cost path search device has been suggested which searches for a path from an entrance node to an exit node with the lowest cost in a network using the Dijkstra method (for example, see Japanese Unexamined Patent Application Publication No. 2002-133351).
The RRT is a tree data structure properly encompassing a motion space and an algorithm serving as a basis for generating the tree. In the RRT algorithm, an initial position is first set in the motion space, a random sampling position is configured in the same motion space, the nearest position between the initial position and the sampling position is searched for, and the initial position and the nearest position are connected to and added to the tree. Thereafter, by repeating the configuration of the sampling position relative to the added nearest position, the additional search for the nearest position, and the addition of the searched nearest position to the tree, it is possible to generate a tree encompassing the motion space. The RRT algorithm is widely used in path planning or path generation in real time (for example, see “Dynamically-stable Motion Planning for Humanoid Robots” (Autonomous Robots vol. 12, No. 1, pp. 105-118, 2002), written by James J. Kuffner) and have been greatly studied.
In the potential method, it is difficult to calculate efficiently a stable motion path, in that there is a possibility that plural positions and postures having the minimum potential may appear in addition to the target position and posture, which is very intuitive. The stochastic road map method has a disadvantage in that it is difficult to plan a motion in a narrow space surrounded with many obstructions, that is, plural positions and postures should be sampled. The RRT algorithm, can enable relatively rapid path planning in a multi-dimensional configuration space, but has a disadvantage that the searched path includes many detours due to the use of random search.
In this way, the past path planning methods have advantages and disadvantages. It is necessary to correct the path generated by one method in order to smooth the path or avoid an obstruction. As a method of smoothing a path, methods using the minimum jerk (for example, see “The Coordination of Arm Movements: An Experimentally Confirmed Mathematical Model” (The Journal of Neuroscience, Vol. 5, No. 7, pp. 1688-1703, 1985), written by T. Flash and N. Hogan), the minimum torque, the polynomial expression (spline) have been suggested.
However, no method can satisfy the requirements of (2) to acquire a smooth position and posture path, (3) to avoid infiltration between objects, and (4) to approach an ideal position and posture path. In other words, it is not possible to obtain a satisfactory path for use in the motion control of a robot or the final control for the path planning of a finger tip of a multi-joint arm.
For example, a method of smoothing a humanoid motion planning path using the B-spline has been suggested (for example, see “Smoothing of Humanoid Motion Planning Path using B-spline” (Proceedings of the 2008 JSME Conference on Robotics and Mechatronics, 2P1-F16, 2008), written by HARADA, MORITAKU, MIURA, HUZIWARA, AND KAZITA). In this method, the correction of path can be carried out using positions in a space and three-degree-of-freedom postures, but there is a problem in that the calculation amount is excessively great.